/*
 * PMMC1.c
 *
 *  Created on: 14/03/2013
 *      Author: RIchard
 */
#include <msp430.h>
#include "../../cdef.h"

void PMM__vSetVCoreUp( uint8 u8Level )
{
    // Open PMM module registers for write access
    PMMCTL0_H = 0xA5;

    // Set SVS/M high side to new level
    SVSMHCTL = SVSHE + SVSHRVL0 * u8Level + SVMHE + SVSMHRRL0 * u8Level;

    SVSMLCTL = SVSLE + SVMLE + SVSMLRRL0 * u8Level;                    // Set SVM new Level
    while ((PMMIFG & SVSMLDLYIFG) == 0) ;                            // Wait till SVM is settled
                                                                     // (Delay)
    PMMCTL0_L = PMMCOREV0 * u8Level;                                   // Set VCore to x
    PMMIFG &= ~(SVMLVLRIFG + SVMLIFG);                               // Clear already set flags
    if ((PMMIFG & SVMLIFG))
        while ((PMMIFG & SVMLVLRIFG) == 0) ;                         // Wait till level is reached

    SVSMLCTL = SVSLE + SVSLRVL0 * u8Level + SVMLE + SVSMLRRL0 * u8Level; // Set SVS/M Low side to new
                                                                     // level
    PMMCTL0_H = 0x00;                                                // Lock PMM module registers
                                                                     // for write access
}

void PMM__vSetVCoreDown( uint8 u8Level )
{
    PMMCTL0_H = 0xA5;                                                // Open PMM module registers
                                                                     // for write access
    SVSMLCTL = SVSLE + SVSLRVL0 * u8Level + SVMLE + SVSMLRRL0 * u8Level; // Set SVS/M Low side to new
                                                                     // level
    while ((PMMIFG & SVSMLDLYIFG) == 0) ;                            // Wait till SVM is settled
                                                                     // (Delay)
    PMMCTL0_L = (u8Level * PMMCOREV0);                                 // Set VCore to 1.85 V for Max
                                                                     // Speed.
    PMMCTL0_H = 0x00;                                                // Lock PMM module registers
                                                                     // for write access
}

void PMM_vSetCore( uint8 u8Level )
{
    uint8 u8ActualLevel;

    do
    {
    	u8ActualLevel = PMMCTL0_L & PMMCOREV_3;
        if (u8ActualLevel < u8Level)
        	PMM__vSetVCoreUp(++u8ActualLevel);                                  // Set VCore (step by step)
        if (u8ActualLevel > u8Level)
        	PMM__vSetVCoreDown(--u8ActualLevel);                                // Set VCore (step by step)
    }
    while (u8ActualLevel != u8Level);
}
